/*
Odd and Even functions
	The Cooley-Tukey FFT algorithm seperates a discrete signal
	into odd funcitons and even functions. In other words it 
	is seperating the sin and cos components of the signal. We
	Then determine the magnitude of the sin and cos components
	at varying frequencies. 

	To determine the magnitude of an even function at a certain
	frequency we add samples together and we multiply odd 
	samples by a twiddle factor (described below).

	A algebraic example is shown below. This example will 
	compute the output of our butterfly diagram for the 
	first 4 samples. Note capital 'X' denotes a frequency
	domain signal, lower case 'x' denotes time domain signal.
	
	X0 = (x0 + W0*x2) + W0*(x1 + W0*x3)
	X1 = (x0 - W0*x2) + W1*(x1 - W0*x3)
	X2 = (x0 + W0*x2) - W0*(x1 + W0*x3)
	X3 = (x0 - W0*x2) - W1*(x1 - W0*x3)

Butterfly Diagram
	As stated above the DFT is recursively reduced. That is 
	an 8 sample DFT is reduced to two 4 sample DFTs and then 
	to four 2 sample DFT's. During each recursion even samples
	are grouped together. 

	When this is drawn out diagramatically the 2 sample DFT's 
	resemble a butterfly; hence the term butterfly diagram.	

	To actually perform the FFT we calculate the four 2 sample
	DFT's first, followed by the two 4 sample DFT's and finally
	the 8 sample DFT.

Twiddle factors 
	Twiddle factors are the complex number co-efficients 
	that are used to multipy samples on the butterfly diagram.

	Each twiddle factor is indexed. The index is associated
	with the complex unit circle (explained further below). 

	Twiddle factors are indexed in bit reverse order. That is
	we initally label the twiddle factors on our butterfly
	diagram according to the type of butterfly diagram. 

	Eight sample DFT's have a range 0-3. This is calculated
	from the equasion below:
		index range = 0 to ((samples/2) - 1);
					= 0 to (8/2)-1
					= 0 to 4 - 1
					= 0 to 3
	Four sample DFT's have a range 0-1.
		index range = 0 to ((samples/2) - 1);
					= 0 to 4/2 - 1
					= 0 to 2 - 1
					= 0 to 1
	
	The calculation is the same for any size DFT.

Calculating Twiddle Factors	
	To calculate a twiddle factor we use the twiddle factors
	index to lookup a value on the complex unit circle. The 
	lookup is done in bitwise reverse ordre. The following 
	example shows how we bitwise reverse the first 4 indexes. 

	00		00
	01	=>	10
	10		01
	11		11

	Once you have bit reversed the index you look up the 
	value of the twiddle factor from the unit circle.

	To lookup a value from the complex unit circle we divide
	the bottom half (bottom semi circle) into, using the 16 
	sample FFT index example from above, 3 slices; 
	3 being the maximum index in the example. Once the 
	bottom complex unit semi circle has 3 equal divisions we
	lookup the bit reversed index in a clockwise direction.

	Let p be the index of our twiddle factor and P is the 
	maximum range of our twiddle factors for the associated
	butterfly diagram. W represents a twiddle factor. Wp is 
	a twiddle factor with index p.
	
	The general formula to calculate Wp is shown below:
		Wp = e^(-j*pi*p/P)

	Normally the general formula is shown as
		W[p,q] = e^(-j*2*pi*q/P)

	EXAMPLES
	For a 4 sample butterfly diagram
	W0 = e^(0) = 1
	W1 = e^(-j*pi*1/2)
	
	for an 8 sample butterfly diagram
	W0 = e^(0) = 1
	W1 = e^(-j*pi*1/4)
	W2 = e^(-j*pi*2/4)
	W3 = e^(-j*pi*3/4)
*/
#include <math.h>
#include <stddef.h>
#include <stdlib.h>

#include "FreeRTOS.h"
#include "task.h"

#define PI 3.1415926536

//A structure to form a complex number
typedef struct
{
	float real;
	float imaginary;
}Complex;

/* char CreateTwiddleFactors(Complex * twiddleFactors, int maxOrder)

Description:
	A function that populates an array of twiddle factors
	these twiddle factors are NOT indexed in bitwise reverse
	format. 

input:	
	twiddleFactors	: a pointer to the first element of a complex structure
	maxOrder		: The number of elements in the twiddle factor array.

return:
	Rreturns an number indicating the success of the function
		0 = Successful
		> 0 fail with error number.
*/
char CreateTwiddleFactors(Complex * twiddleFactors, int maxOrder);

/* char FFT (int * data, Complex * out, int NbSamples)

Description:
	A function that performs an FFT 

input:	
	In			: A pointer to the first element of the data input in the FFT
	Out			: A pointer to the first element of the data output from the FFT
	NbSamples	: The number of samples in the input and outp array's


return:
	Rreturns an number indicating the success of the function
		0 = Successful
		> 0 fail with error number.
*/
char FFT (int * data, Complex * out, int NbSamples);

/* char bitReverse(int * lookupTable, char indexLen)
Description:
	Creates a lookup table with bitwise reversed indexes.
	An example if indexLen = 4
	
	Normal Index		Bitwise Reversed Index
		00						00
		01			=>			10
		10						01
		11						11
input:
	lookupTable	: A pointer to the first element of a lookup table that will be used to lookup a bitwise reversed index
	indexLen	: The number of indexes in the lookupTable

return:
	Rreturns an number indicating the success of the function
		0 = Successful
		> 0 fail with error number.
*/
char bitReverse(int * lookupTable, char indexLen);

char CreateTwiddleFactors(Complex * twiddleFactor, int maxOrder)
{
	int q; //An index to the twiddle factor
	int P; //The current maximum order of the twiddle factor
	float w = 0;	//Omega (in eulers formula e^(jw) = cos(w) + jsin(w)
	
	//Request memory from the heap to stor twiddle factors
	vTaskSuspendAll();
	{
		twiddleFactor = (Complex *) malloc(maxOrder* sizeof(Complex));
	}
	xTaskResumeAll();
	
	if(twiddleFactor == NULL)
		return 1; //Failed to get enough memory from the heap

	//Twiddle factors have a bitwise reverse index
	for(P = 1; P<= maxOrder; P++)
	{
		for(q = 0; q<P; q++)
		{
			w = PI*(q/P);
			twiddleFactor[q*P].real = cos(w);
			twiddleFactor[q*P].imaginary = -1 * sin(w);
		}	
	}
	return 0; //function completed successfully
}

/*

inputs:
		Order: Twiddle Factor order
		index: The Twiddle factor index
		*/

char GetTwiddleFactor(Complex * twiddleFactor, int order, int index)
{
	float w = 0;	//Omega (in eulers formula e^(jw) = cos(w) + jsin(w)
	w = PI*(index/order);
	twiddleFactor->real = cos(w);
	twiddleFactor->imaginary = -1 * sin(w);
}

//index Len is the number of bits required to store the indexes
//
char bitReverse(int * lookupTable, char indexLen)
{	
	int x;
	int y;	
	//Request memory from the heap to fill the array.
	vTaskSuspendAll();
	{
		lookupTable = (int *)malloc( (1 << indexLen) * sizeof(int));
	}
	xTaskResumeAll();
	
	if(lookupTable == NULL)
		return 1; //Failed to get enough memory from the heap

	lookupTable[0] = 0;	//First element is always zero
	indexLen--;			//There is n bits in the number but for bit minipulation purposes we treat this a n-1 bits

	for(x = 1; x<indexLen-1; x++)	//Loop through each element
	{
		for(y = indexLen; y>=0; y--)//loop through each bits from msb to lsb
		{
			if(lookupTable[x-1] & 1 << y)//If current bit is set
			{
				lookupTable[x] = lookupTable[x-1] & ~(1 << y); 	//Clear the current bit
				//go to next bit and see if it is set
			}
			else
			{
				lookupTable[x] = lookupTable[x-1] | (1 << y);	//Set the current bit
				break;	//break the current loop go to next element
			}
		}		
	}	
	return 0;	//Return success	
}

char FFT (int * data, Complex * out, int NbSamples)
{
	int * lookupBitwiseReverse;
	Complex * twiddleFactor;
	char returnVar = 0;
	signed char oddEvenFactor = 0;
	char CurrentTwiddleFactorRange = 0;
	char oddEvenCounter = 0;	/*	The twiddle factor sign has to be swapped at different times during the algorithm. 
									The frequency of sign switching is determined by the type of twiddle factor we are using.
									For example: 	a 2nd order twiddle factor switches the sign every two cycles
													a 3rd order twiddle factor switches the sign every four cycles
									in general the sign is switched at a frequency of: 
													2 ^ (twiddle factor order)										*/
	int y = 0;					//Y indices the twiddle factor order we are using
	int x = 0;					//x is a indacy to the data.
	
	//NbSamples needs to be a power of 2
	//	loop through each bit of NbSamples ensuring only 1 bit is set
	for(x = 1; x< sizeof(NbSamples)*8; x++)
	{
		if(NbSamples & 1 << x)
		{
			bitReverse(lookupBitwiseReverse, x);	//Optimisation: calculate the bitwise reverse order in this loop because it tells us how many bits we need to reverse
			if(returnVar !=0)
				return returnVar;
			returnVar++; 							//Optimisation using already initialised memory so we dont need to create a new variable for this check
		}
	}
	
	if(returnVar == 1)	//If only one bit has been set 
		returnVar = 0;	//Re-Initialise the variable
	else
		return returnVar;//Our key assumption has not been met we cannot perform an FFT unles NbSamples = 2^n where n = 2-16
	
	//Create an array to store twiddle factors
	returnVar = CreateTwiddleFactors(twiddleFactor, NbSamples - 1);
	if(returnVar !=0)
	{
		free(lookupBitwiseReverse);
		return 3;
	}	
	
/************FFT PART 1***********************/
	//The first part of the FFT algorithm is an optimisation that this done for two reasons 
	//	1. We loop through and initialise the output
	//	2. Since the first loop of the FFT is going to multiply the data by a twiddle factor of 1 
	//		we dont need to multiply by a twiddle factor. This also means we dont need to compute
	//		an imaginary component.
	
	/*****************OPTIMISATION 1 *************/	
		//We initialise the imaginary component to 0 during this loop.
		for(x = 0; x < NbSamples; x++)//Loop through each sample
		{
		 	out[x].real = data[lookupBitwiseReverse[x]] + 
					oddEvenFactor*data[lookupBitwiseReverse[x+1]];
			
			//Initialise the imaginary component (incase the OS gave us a block of uninitialised memory)
			out[x].imaginary = 0;
			
			oddEvenFactor ^= 0x8000;	//Toggling the msb of a signed int swaps the sign
		}
	/*****************END OPTIMISATION 1*************/

/************END FFT PART 1***********************/

/************FFT PART 2***********************/
	//During FFT part 2 we are re-using the same memory for each itteration.
	//	If we evaluate the FFT one itteration at a time (one twiddle factor order at a time)
	//	we dont need to store interum calculations.
	// 	This is why we are using += instead of = in calculate_FFT


	//Y tells us how many equasions to evaluate before changing the sign
	for(y = 1; y <= NbSamples; (y = y << 1)-1)//Loop through each set of butterfly diagrams ( y = 2^n-1 )
	{
		CurrentTwiddleFactorRange = (1 << y) -1;
	/*****************OPTIMISATION 2 *************/
		//The first itteration of each twiddle factor order can be optimised because the twiddle factor = 1.
		//That is the real component is multiplied by 1 and the imaginary component is multiplied by 0;
		//	leaving the real component as the sum of the two data inputs and the imaginary component unchanged.
		out[0].real = data[0] + data[lookupBitwiseReverse[y+1]];
		
		//Imaginary component remains unchanged

		oddEvenCounter++;
		if(oddEvenCounter == CurrentTwiddleFactorRange){
			/*By dynamically creating the twiddle factor we will save half the amount of memory used by the function*/
			
			oddEvenFactor ^= 0x80;		//Toggling the msb of a signed char swaps the sign
			oddEvenCounter = 0;			//Reset the counter
		}
	/*****************END OPTIMISATION 2 *************/

		//Due to optimisation 2 (above) we have started this loop at 1 instead of 0.
		for(x = 1; x < NbSamples; x++)//Loop through each sample
		{


			/*In general the FFT is comprised of a sum of two terms (x[n] +- W[y] * x[n+(y-1)])
				Where: 
						n 			is a bitwise reversed index. 
						y 			is indicates the twiddle factor order
						W[y] 		is a twiddle factor
						+-			is the sign used for the current itteration (either a + or a - is used) 
						x[n]		is the data
						n + (y-1)	is a bitwise recersed index that is offset by the twiddel factor order
			*/			
				if(oddEvenFactor & 0x80)	//If factor is odd
				{
					//We are using the following formula is used for complex multiplication
					//		(a + jb)*(c+jd) = ac -bd + jad + jbc
					//In general the entire equation is
					//		Xn = (xa + Wn*xb)
					//		where:
					//			Xn is a single FFT calculation
					//			xa is the first input
					//			xb is the second intpu
					//			Wn is the twiddle factor
					out[x].real 	+= 	out[lookupBitwiseReverse[x]].real +									// 	xa.real
							 			(twiddleFactor[y*oddEvenCounter].imaginary *							//	Wn.imaginary
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].imaginary) -	//	xb.imaginary
										(twiddleFactor[y*oddEvenCounter].real *									//	Wn.real
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].real);			// 	xb.real

					out[x].imaginary+= 	out[lookupBitwiseReverse[x]].imaginary +								//	xa.real
										(twiddleFactor[y*oddEvenCounter].imaginary *							//	Wn.imaginary
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].real)*-1 -		// -xb.real
										(twiddleFactor[y*oddEvenCounter].real *									//	Wn.real
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].imaginary); 	//	xb.imaginary
				}
				else
				{
					//We are using the following formula is used for complex multiplication
						//-1*((a + jb)*(c+jd)) = bd -ac - jad - jbc
					//In general the entire equation is
					//		Xn = (xa - Wn*xb)
					//		where:
					//			Xn is a single FFT calculation
					//			xa is the first input
					//			xb is the second intpu
					//			Wn is the twiddle factor
					out[x].real 	+= 	out[lookupBitwiseReverse[x]].real +									// 	xa.real
										(twiddleFactor[y*oddEvenCounter].real * 							//	Wn.real
									 	out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].real)- 	//	xb.real
							 			(twiddleFactor[y*oddEvenCounter].imaginary *						//	Wn.imaginary
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].imaginary);//	xb.imaginary

					out[x].imaginary+= 	out[lookupBitwiseReverse[x]].imaginary +							//	xa.imaginary
										(twiddleFactor[y*oddEvenCounter].real	*							//	Wn.real
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].imaginary)+//	xb.imaginary
										(twiddleFactor[y*oddEvenCounter].imaginary *						//	Wn.imaginary
										out[lookupBitwiseReverse[x+1]+CurrentTwiddleFactorRange].real);		//	xb.real
				}

			oddEvenCounter++;
			if(oddEvenCounter == CurrentTwiddleFactorRange){
				oddEvenFactor ^= 0x80;	//Toggling the msb of a signed char swaps the sign
				oddEvenCounter = 0;			//Reset the counter
			}
		}
	}
/************END FFT PART 2***********************/
	free(lookupBitwiseReverse);
	free(twiddleFactor);
	return 0;	//Function completed successfully
}
